#include <bags_record/bag_record_node.h>

// 定义订阅者模版
// 参考 https://gitee.com/caojiangwei/cartographer_ros/blob/master/cartographer_ros/cartographer_ros/node.cc
template <typename MessageType>
::ros::Subscriber SubscribeWithHandler(
    // 定义函数指针
    void (BagsRecord::*handler)(BagTopic, const typename MessageType::ConstPtr&),
    // 参数传递
    BagTopic bag_topic, ::ros::NodeHandle* const node_handle, BagsRecord* const bag_record) 
{
    // 构造订阅者，并返回
    return node_handle->subscribe<MessageType>(bag_topic.topic_name, 1000,
        // 定义处理函数，并传参
        boost::function<void(const typename MessageType::ConstPtr&)>(
            [bag_record, handler, bag_topic](const typename MessageType::ConstPtr& msg) { 
                (bag_record->*handler)(bag_topic, msg);
            }
        )
    );
};


// 初始化订阅者
void BagsRecord::SubscriberInit(){
    ROS_INFO_STREAM("[SubscriberInit]......");
    if(bagTopicsVec_.empty()) return;
    for(auto bag_topic : bagTopicsVec_){
        if(bag_topic.disable) continue;
        // 更新参数
        bag_topic.max_time_limit = __bags_data_save_time + __bags_delay_save_time;   //单位秒
        if(bag_topic.save_hz > 0)
            bag_topic.time_interval = 1.0 / bag_topic.save_hz;
        ROS_INFO("[SubscriberInit] topic_name: %s init......", bag_topic.topic_name.c_str());
        // 创建缓存数据
        string topic_name = bag_topic.topic_name;
        // 处理雷达类型消息
        if(bag_topic.data_format == "sensor_msgs/LaserScan"){
            subList_[topic_name] = SubscribeWithHandler<sensor_msgs::LaserScan>(
                &BagsRecord::HandleLaserScanMessage, bag_topic, &__nh, this
            );
        }  
        // 处理地图类型消息
        if(bag_topic.data_format == "nav_msgs/OccupancyGrid"){
            subList_[topic_name] = SubscribeWithHandler<nav_msgs::OccupancyGrid>(
                &BagsRecord::HandleOccupancyGridMessage, bag_topic, &__nh, this
            );
        }
        // 处理Path类型消息
        if(bag_topic.data_format == "nav_msgs/Path"){
            subList_[topic_name] = SubscribeWithHandler<nav_msgs::Path>(
                &BagsRecord::HandlePathMessage, bag_topic, &__nh, this
            );
        }
        // 处理String类型消息
        if(bag_topic.data_format == "std_msgs/String"){
            subList_[topic_name] = SubscribeWithHandler<std_msgs::String>(
                &BagsRecord::HandleStringMessage, bag_topic, &__nh, this
            );
        }
        // 处理Imu类型消息
        if(bag_topic.data_format == "sensor_msgs/Imu"){
            subList_[topic_name] = SubscribeWithHandler<sensor_msgs::Imu>(
                &BagsRecord::HandleImuMessage, bag_topic, &__nh, this
            );
        }
        // 处理Odometry类型消息
        if(bag_topic.data_format == "nav_msgs/Odometry"){
            subList_[topic_name] = SubscribeWithHandler<nav_msgs::Odometry>(
                &BagsRecord::HandleOdometryMessage, bag_topic, &__nh, this
            );
        }
        // 处理TFMessage类型消息
        if(bag_topic.data_format == "tf2_msgs/TFMessage"){
            subList_[topic_name] = SubscribeWithHandler<tf2_msgs::TFMessage>(
                &BagsRecord::HandleTFMessageMessage, bag_topic, &__nh, this
            );
        }
    } 

} 

// 处理雷达类型消息
void BagsRecord::HandleLaserScanMessage(BagTopic bag_topic, const sensor_msgs::LaserScan::ConstPtr& msg){

}

// 处理地图类型消息
void BagsRecord::HandleOccupancyGridMessage(BagTopic bag_topic, const nav_msgs::OccupancyGrid::ConstPtr& msg){

}

// 处理Path类型消息
void BagsRecord::HandlePathMessage(BagTopic bag_topic, const nav_msgs::Path::ConstPtr& msg){

}

// 处理String类型消息
void BagsRecord::HandleStringMessage(BagTopic bag_topic, const std_msgs::String::ConstPtr& msg){

}

// 处理Imu类型消息
void BagsRecord::HandleImuMessage(BagTopic bag_topic, const sensor_msgs::Imu::ConstPtr& msg){

}

// 处理Odometry类型消息
void BagsRecord::HandleOdometryMessage(BagTopic bag_topic, const nav_msgs::Odometry::ConstPtr& msg){

}

// 处理TFMessage类型消息
void BagsRecord::HandleTFMessageMessage(BagTopic bag_topic, const tf2_msgs::TFMessage::ConstPtr& msg){
    
}